import { Vector2, Vector3 } from '../utils/Math';
import { Polyhedron3D } from '../collision/Collision';

/**
 * 通用RVO（Reciprocal Velocity Obstacles）避障管理器，支持2D/3D
 * 用法：
 *   // 2D
 *   const rvo2d = new RVOManager('2d');
 *   const id = rvo2d.addAgent({ position: new Vector2(0,0), radius: 0.5, maxSpeed: 1, goal: new Vector2(10,0) });
 *   rvo2d.addObstacle({ p1: new Vector2(0,0), p2: new Vector2(10,0) });
 *   rvo2d.update();
 *   // 3D
 *   const rvo3d = new RVOManager('3d');
 *   const id3 = rvo3d.addAgent({ position: new Vector3(0,0,0), radius: 0.5, maxSpeed: 1, goal: new Vector3(10,0,0) });
 *   rvo3d.addObstacle({ point: new Vector3(0,0,0), normal: new Vector3(0,1,0) });
 *   rvo3d.update();
 */

type Mode = '2d' | '3d';
type Vector = Vector2 | Vector3;

// 2D Agent/Obstacle
export interface Agent2DOptions {
    position: Vector2;
    radius: number;
    maxSpeed: number;
    velocity?: Vector2;
    weight?: number;
    goal?: Vector2;
    neighborDist?: number;
    timeHorizon?: number;
}
export interface Obstacle2D { p1: Vector2; p2: Vector2; }

// 3D Agent/Obstacle
export interface Agent3DOptions {
    position: Vector3;
    radius: number;
    maxSpeed: number;
    velocity?: Vector3;
    weight?: number;
    goal?: Vector3;
    neighborDist?: number;
    timeHorizon?: number;
}
export interface ObstaclePlane3D { point: Vector3; normal: Vector3; }
export interface ObstaclePolyhedron3D extends Polyhedron3D {}

// 通用Agent（无范型，直接用联合类型）
class Agent {
    id: number;
    position: any;
    velocity: any;
    prefVelocity: any;
    radius: number;
    maxSpeed: number;
    weight: number;
    goal?: any;
    neighborDist: number;
    timeHorizon: number;
    constructor(id: number, opts: any, zero: any) {
        this.id = id;
        this.position = opts.position.clone();
        this.velocity = opts.velocity ? opts.velocity.clone() : zero.clone();
        this.prefVelocity = zero.clone();
        this.radius = opts.radius;
        this.maxSpeed = opts.maxSpeed;
        this.weight = opts.weight ?? 1.0;
        this.goal = opts.goal ? opts.goal.clone() : undefined;
        this.neighborDist = opts.neighborDist ?? 1.0;
        this.timeHorizon = opts.timeHorizon ?? 5.0;
    }
    updatePrefVelocity(zero: any) {
        if (this.goal) {
            const toGoal = this.goal.sub(this.position);
            if (toGoal.length() > 0.01) {
                this.prefVelocity = toGoal.normalize().mul(this.maxSpeed);
            } else {
                this.prefVelocity = zero.clone();
            }
        }
    }
}

export class RVOManager {
    private agents: Map<number, Agent> = new Map();
    private nextId = 1;
    private config: { neighborDist: number; timeHorizon: number; timeStep: number } = {
        neighborDist: 1.0,
        timeHorizon: 5.0,
        timeStep: 0.05,
    };
    // 2D
    private obstacles2D: Obstacle2D[] = [];
    // 3D
    private obstaclesPlane3D: ObstaclePlane3D[] = [];
    private obstaclesPoly3D: ObstaclePolyhedron3D[] = [];
    private mode: Mode;
    constructor(mode: Mode, config?: Partial<{ neighborDist: number; timeHorizon: number; timeStep: number }>) {
        this.mode = mode;
        if (config) Object.assign(this.config, config);
    }
    /**
     * 添加Agent
     * @param opts Agent参数（2D/3D自动分派）
     * @returns agent id
     */
    addAgent(opts: Agent2DOptions | Agent3DOptions): number {
        const id = this.nextId++;
        if (!opts.neighborDist) opts.neighborDist = this.config.neighborDist;
        if (!opts.timeHorizon) opts.timeHorizon = this.config.timeHorizon;
        let agent;
        if (this.mode === '2d') {
            agent = new Agent(id, opts, (Vector2.zero as any));
        } else {
            agent = new Agent(id, opts, (Vector3.zero as any));
        }
        this.agents.set(id, agent);
        return id;
    }
    /**
     * 移除Agent
     */
    removeAgent(id: number) { this.agents.delete(id); }
    /**
     * 设置Agent期望速度
     */
    setAgentPrefVelocity(id: number, v: Vector) {
        const agent = this.agents.get(id);
        if (agent) agent.prefVelocity = v.clone();
    }
    /**
     * 设置Agent目标点
     */
    setAgentGoal(id: number, goal: Vector) {
        const agent = this.agents.get(id);
        if (agent) agent.goal = goal.clone();
    }
    /**
     * 获取Agent
     */
    getAgent(id: number) { return this.agents.get(id); }
    /**
     * 获取所有Agent
     */
    getAllAgents() { return Array.from(this.agents.values()); }
    /**
     * 添加障碍物（2D线段/3D平面/3D多面体）
     */
    addObstacle(obstacle: Obstacle2D | ObstaclePlane3D | ObstaclePolyhedron3D) {
        if (this.mode === '2d' && (obstacle as Obstacle2D).p1) {
            this.obstacles2D.push(obstacle as Obstacle2D);
        } else if (this.mode === '3d' && (obstacle as ObstaclePlane3D).normal) {
            this.obstaclesPlane3D.push(obstacle as ObstaclePlane3D);
        } else if (this.mode === '3d' && (obstacle as ObstaclePolyhedron3D).vertices) {
            this.obstaclesPoly3D.push(obstacle as ObstaclePolyhedron3D);
        }
    }
    /**
     * 清空所有障碍物
     */
    clearObstacles() {
        this.obstacles2D = [];
        this.obstaclesPlane3D = [];
        this.obstaclesPoly3D = [];
    }
    /**
     * RVO主循环，更新所有Agent位置和速度
     * @remarks
     *  - 2D支持线段障碍物，3D支持平面/多面体障碍物
     *  - 可每帧调用
     */
    update() {
        const agents = this.getAllAgents();
        const newVelocities: Map<number, Vector> = new Map();
        for (const agent of agents) {
            agent.updatePrefVelocity(this.mode === '2d' ? (Vector2.zero as any) : (Vector3.zero as any));
        }
        for (const agent of agents) {
            const neighbors = agents.filter(a => a.id !== agent.id && (agent.position as any).distanceSq(a.position as any) < agent.neighborDist * agent.neighborDist);
            let orcaVelocity = agent.prefVelocity.clone();
            // 避让其他Agent
            for (const neighbor of neighbors) {
                const relPos = (neighbor.position as any).sub(agent.position as any);
                const relVel = (agent.velocity as any).sub(neighbor.velocity as any);
                const distSq = relPos.lengthSq();
                const combinedRadius = agent.radius + neighbor.radius;
                const combinedRadiusSq = combinedRadius * combinedRadius;
                const t = Math.min(agent.timeHorizon, neighbor.timeHorizon);
                if (distSq < combinedRadiusSq) {
                    orcaVelocity = orcaVelocity.add(relPos.normalize().mul(agent.maxSpeed));
                } else {
                    const w = relVel.sub(relPos.mul(1 / t));
                    const wLen = w.length();
                    if (wLen < agent.maxSpeed) {
                        orcaVelocity = orcaVelocity.sub(w.normalize().mul(agent.maxSpeed - wLen));
                    }
                }
            }
            // 避让障碍物
            if (this.mode === '2d') {
                for (const obstacle of this.obstacles2D) {
                    const dist = this.distanceToObstacle2D(agent, obstacle);
                    if (dist < agent.radius * 2) {
                        const { p1, p2 } = obstacle;
                        const ap = (agent.position as any).sub(p1);
                        const ab = p2.sub(p1);
                        const t = Math.max(0, Math.min(1, ap.dot(ab) / ab.lengthSq()));
                        const closest = p1.add(ab.mul(t));
                        const away = (agent.position as any).sub(closest).normalize();
                        orcaVelocity = orcaVelocity.add(away.mul(agent.maxSpeed));
                    }
                }
            } else {
                for (const plane of this.obstaclesPlane3D) {
                    const dist = this.distanceToPlane3D(agent, plane);
                    if (dist < agent.radius * 2) {
                        orcaVelocity = orcaVelocity.add((plane.normal as any).mul(agent.maxSpeed));
                    }
                }
                for (const poly of this.obstaclesPoly3D) {
                    const dist = this.distanceToPolyhedron3D(agent, poly);
                    if (dist < agent.radius * 2) {
                        let minDist = Infinity;
                        let bestNormal = (Vector3.zero as any);
                        for (const face of poly.faces) {
                            const v0 = poly.vertices[face[0]];
                            const v1 = poly.vertices[face[1]];
                            const v2 = poly.vertices[face[2]];
                            const normal = v1.sub(v0).cross(v2.sub(v0)).normalize();
                            const distFace = Math.abs((agent.position as any).sub(v0).dot(normal));
                            if (distFace < minDist) {
                                minDist = distFace;
                                bestNormal = normal;
                            }
                        }
                        orcaVelocity = orcaVelocity.add(bestNormal.mul(agent.maxSpeed));
                    }
                }
            }
            if (orcaVelocity.length() > agent.maxSpeed) {
                orcaVelocity = orcaVelocity.normalize().mul(agent.maxSpeed);
            }
            newVelocities.set(agent.id, orcaVelocity);
        }
        for (const agent of agents) {
            const v = newVelocities.get(agent.id) ?? agent.prefVelocity;
            agent.velocity = v.clone();
            if (this.mode === '2d') {
                agent.position = (agent.position as any).add((agent.velocity as any).mul(this.config.timeStep));
            } else {
                agent.position = (agent.position as any).add((agent.velocity as any).mul(this.config.timeStep));
            }
        }
    }
    // 2D辅助
    private distanceToObstacle2D(agent: any, obstacle: Obstacle2D): number {
        const { p1, p2 } = obstacle;
        const ap = (agent.position as any).sub(p1);
        const ab = p2.sub(p1);
        const t = Math.max(0, Math.min(1, ap.dot(ab) / ab.lengthSq()));
        const closest = p1.add(ab.mul(t));
        return (agent.position as any).distance(closest);
    }
    // 3D辅助
    private distanceToPlane3D(agent: any, plane: ObstaclePlane3D): number {
        return Math.abs((agent.position as any).sub(plane.point).dot(plane.normal));
    }
    private distanceToPolyhedron3D(agent: any, poly: ObstaclePolyhedron3D): number {
        let minDist = Infinity;
        for (const face of poly.faces) {
            const v0 = poly.vertices[face[0]];
            const v1 = poly.vertices[face[1]];
            const v2 = poly.vertices[face[2]];
            const normal = v1.sub(v0).cross(v2.sub(v0)).normalize();
            const dist = Math.abs((agent.position as any).sub(v0).dot(normal));
            if (dist < minDist) minDist = dist;
        }
        return minDist;
    }
} 